// BulletTestApp.cpp : Defines the entry point for the console application.
//

//#include "stdafx.h"

#include "BulletExporter.pch"
#include "btBulletDynamicsCommon.h"
#include "BulletCollision/CollisionDispatch/btCylinderCylinderCollisionAlgorithm.h"

/*int main()
{
	btVector3 Extent(1,1,1);
	float mass = 1.0f;

	unsigned int id,cid;
	float X, Y, Z;
	float rX, rY, rZ, rW;

	createDiscreteDynamicsWorldWithDbvt(9.8, 4, 1);
	createBoxShape(Extent.x()/2.0f, Extent.y()/2.0f, Extent.z()/2.0f,cid);
	createRigidBody(0,0,0,0,0,0,0,mass,id,cid);

	//simulation(Time.fixedDeltaTime);
	//getPosition(&X, &Y, &Z, id);
	//transform.position = new Vector3(X,Y,Z);
	//print (X+" "+Y+" "+Z);
	//getRotation(out rX, out rY, out rZ, out rW, id);
	//transform.rotation = new Quaternion(rX,rY,rZ,rW);
	//print (X+" "+Y+" "+Z+" "+W);

	return 0;
}*/

btDefaultCollisionConfiguration* collisionConfiguration;
btCollisionDispatcher* dispatcher;
btDynamicsWorld* discreteDynamicsWorld;
btBroadphaseInterface* broadPhase;
btSequentialImpulseConstraintSolver* solver;

float f1, f2, f3, f4, f5, f6, f7;

enum btBroadPhase
{
	btDbvt = 1,
	btAS3,
	btSimple
};

void createDiscreteDynamicsWorldWithDbvt(float gravity, int solverIteration, int _broadPhase)
{
	//if(!discreteDynamicsWorld)
	//{

		
		if (_broadPhase == (int)btDbvt)
		{
			broadPhase = new btDbvtBroadphase();	
		}else if (_broadPhase == (int)btAS3)
		{
			int  maxProxies = 1024;
			btVector3 worldMin(-1000,-1000,-1000);
			btVector3 worldMax(1000,1000,1000);
			broadPhase = new btAxisSweep3(worldMin,worldMax);
		}else
		{
			broadPhase = new btSimpleBroadphase();
		}
		

		
	if(!discreteDynamicsWorld)
	{
		collisionConfiguration = new btDefaultCollisionConfiguration();
		dispatcher = new btCollisionDispatcher(collisionConfiguration);
		solver = new btSequentialImpulseConstraintSolver();
		discreteDynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadPhase,solver,collisionConfiguration);
	}
	else
	{
		if(_broadPhase != btAS3)
			discreteDynamicsWorld->setBroadphase(broadPhase);
	}
		discreteDynamicsWorld ->setGravity(btVector3(0, gravity, 0));
		discreteDynamicsWorld ->getSolverInfo().m_numIterations = solverIteration;
	//}
}

unsigned long simulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
	btClock	clock;
	clock.reset();
	discreteDynamicsWorld->stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
	unsigned long time = clock.getTimeMilliseconds();
	return time;
}

void createBoxShape(float halfExtentX, float halfExtentY, float halfExtentZ, unsigned int& id)
{
	btCollisionShape* shape = new btBoxShape(btVector3(halfExtentX, halfExtentY, halfExtentZ));
	id = (unsigned int)shape;
}

void createCylinderShape(float radius, float height, unsigned int& id)
{
	//btCollisionShape* shape = new btCylinderShape(btVector3(radius, height, radius));
	btCollisionShape* shape = new btCylinderShapeZ(btVector3(radius, radius, height));
	id = (unsigned int)shape;
}

void createRigidBody(float X, float Y, float Z, float rX, float rY, float rZ, float rW, float mass, float restitution, float linearDamping, float angularDamping, float friction, float linearThreshold, float angularThreshold, unsigned int& id, unsigned int colliderId)
{
	//if(!shape)
	//	shape = new btBoxShape(btVector3(0, 0, 0));
	
	btTransform trans;
	trans.setIdentity();
	trans.setOrigin(btVector3(X, Y, Z));
	trans.setRotation(btQuaternion(rX, rY, rZ, rW));
	btVector3 localInertia(0,0,0);
	btCollisionShape* shape = (btCollisionShape*)colliderId;
	shape->calculateLocalInertia(mass,localInertia);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, 0, shape, localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	
	//if(!discreteDynamicsWorld)
	//	createDiscreteDynamicsWorldWithDbvt();
	
	body->setWorldTransform(trans);
	discreteDynamicsWorld->addRigidBody(body);
	id = (unsigned int)body;

    //Physical Material
    body->setRestitution(restitution);
    body->setDamping(linearDamping, angularDamping);
    body->setFriction(friction);
    body->setSleepingThresholds(linearThreshold, angularThreshold);
    
    //debug use
    //f1 = body->getCenterOfMassPosition().x();
    //f2 = body->getCenterOfMassPosition().y();
    //f3 = body->getCenterOfMassPosition().z();
}



void traceLog(float& d1, float& d2, float& d3)
{
    d1 = f1;
    d2 = f2;
    d3 = f3;
}

void getOrigin(float& X, float& Y, float& Z, unsigned int id)
{
	btRigidBody* pBody = (btRigidBody*)id;
    //btVector3 origin = pBody->getCenterOfMassPosition();
	btVector3 origin = pBody->getWorldTransform().getOrigin();
		
	X = origin.getX();
	Y = origin.getY();
	Z = origin.getZ();
}

void getRotation(float& X, float& Y, float& Z, float& W, unsigned int id)
{

	btRigidBody* pBody = (btRigidBody*)id;
	btQuaternion rotation = pBody->getWorldTransform().getRotation();
	
	X = rotation.getX();
	Y = rotation.getY();
	Z = rotation.getZ();
	W = rotation.getW();
}

void exitPhysics()
{

	int i;
	for (i=discreteDynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = discreteDynamicsWorld->getCollisionObjectArray()[i];
		discreteDynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete discreteDynamicsWorld;
	//delete solver;
	//delete broadPhase;
	//delete dispatcher;
	//delete collisionConfiguration;
}